#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>

#include "body.h"

static unsigned int bcount;

static double getDistance(Body *b1, Body *b2)
{
	double dx, dy;
	dx = abs(b1->posx - b2->posx);
	dy = abs(b1->posy - b2->posy);

	return sqrt(dx*dx + dy*dy);
}

Body::Body()
{
	//std::cout << "*";

	bcount++;
	this->id = bcount;

	this->dt = 1;
	this->mass = 1;
	this->posx = bcount;
	this->posy = bcount;
	//std::cout << this->posx << ", " << this->posy << std::endl;
	this->velx = 0;
	this->vely = 0;
}

Body::~Body()
{
	//std::cout << "#";
	bcount--;
}

void
Body::calcNewPos(Body *nbodys)
{
	unsigned int i;
	double m1,m2,r,r0x,r0y;
	
	double Fx,Fy, ax,ay, vx,vy, sx,sy;

	for (i=0; i<bcount; i++)
	{
		if (i == this->id) continue; // skip self
		
		r = getDistance(this, &nbodys[i]);
		if (r == 0.0) continue;

		r0x = this->posx - nbodys[i].posx;
		r0y = this->posy - nbodys[i].posy;
		
		m1 = this->mass;
		m2 = nbodys[i].mass;
		Fx = (-1*GRAVCONST)*(((m1 * m2)/(r*r))*r0x);
		Fy = (-1*GRAVCONST)*(((m1 * m2)/(r*r))*r0y);

		ax = Fx / this->mass;
		ay = Fy / this->mass;

		vx = this->velx + (ax * this->dt);
		vy = this->vely + (ay * this->dt);
 
 		this->velx = vx;
		this->vely = vy;
 
		sx = vx * this->dt;
		sy = vy * this->dt;
		
		this->posx += sx;
		this->posy += sy;
		
#if 0
		std::cout << "r=" << r << std::endl;
		std::cout << "Fx=" << Fx << std::endl;
		std::cout << "Fy=" << Fy << std::endl;
		std::cout << "ax=" << ax << std::endl;
		std::cout << "ay=" << ay << std::endl;
		std::cout << "vx=" << vx << std::endl;
		std::cout << "vy=" << vy << std::endl;
		std::cout << "sx=" << sx << std::endl;
		std::cout << "sy=" << sy << std::endl;
#endif
	}
}

int
Body::getBodyCount(void)
{
	return bcount;
}

void
Body::setMass(double mass)
{
	this->mass = mass;
}

void
Body::setDt(double dt)
{
	this->dt = dt;
}

void
Body::setPos(double x, double y)
{
	this->posx = x;
	this->posy = y;
}

